/*
 * ping_pong.c
 *
 * Created: 18.09.2012 08:45:09
 *  Author: mariuvi
 */ 

#define F_CPU 4915200
#define F_CPU_DIV64 76800


#include "SRAM.h"
#include "USART.h"
#include "CAN.h"
#include "LEDS.h"
#include "definitions.h"

#include <stdlib.h>
#include <string.h>
#include <avr/io.h>
#include <avr/delay.h>
#include <avr/interrupt.h>

typedef enum {INIT, IDLE,SRAM,TRACK}state_case;

int main(void)
{
	unsigned char USART_msg = 0;
	int USART_posx = -1;
	int USART_posy = -1;
	CAN_message_t *msg = NULL;
	CAN_message_t *recv_msg = calloc(1, sizeof(CAN_message_t));
	CAN_message_t send_msg;

	state_case state = INIT; 
		
	sei();
    while(1)
    {	
		//If there's a CAN message waiting, receive it.
		if(CAN_msg_rcvd)
		{
			msg = CAN_message_receive();
			memcpy(recv_msg, msg, sizeof(CAN_message_t));
			free(msg);

		}

		switch(recv_msg->id)
		{
			case CAN_SRAM_TEST_MSG:
				state = SRAM;
				break;
			case CAN_TRACK_MSG:
				state = TRACK;
				LEDS_init();
				break;
			case CAN_LEDS_INIT_MSG:
				LEDS_init();
				break;
			case CAN_LEDS_DEC_MSG:
				LEDS_decrement();
				break;					
		}
		
		switch(state)
		{
			case INIT:
				USART_init(9600);
				SRAM_init();
				CAN_init();				
				state = IDLE;				
				break;
			
			case IDLE:
				break;
			
			case SRAM:
				SRAM_test();
				state = IDLE;
				break;
			
			case TRACK:
				if(USART_msg_rcvd)
				{
					USART_msg = USART_receive();
					if(!BIT_CHECK(USART_msg, 7))
					{
						USART_posx = (USART_msg & 0b01111111);
						
						//Need saturation before scaling, since the reg's 100% != ball's 100%
						
						//Scale USART_posx, -100% to 100% 	
						USART_posx = ((63 - USART_posx)*200/127);
					}
					else
					{
						USART_posy = (USART_msg & 0b01111111);
					}
					
					if((USART_posx != -1)  && (USART_posy != -1) )
					{
						//Received both x and y coordinates, send this info to the PID
						send_msg.id = CAN_TRACK_CONTROL_MSG;
						send_msg.data[0] = (int8_t)USART_posx;
						send_msg.data[1] = 0;
						send_msg.data[2] = 0;
						
						//Check if we want to trigger the solenoid
						if(USART_posy < 17)
						{
							send_msg.data[3] = 1;
						}
						else
						{
							send_msg.data[3] = 0;
						}						
						send_msg.length = 4;
						
						CAN_message_send(send_msg);
						
						USART_posx = -1;
						USART_posy = -1;
					}
				}
				break;
		}
		recv_msg->id = 255;				
	}		
}

